//------------------------------------------------------------------------------ 
//  <copyright file="EstimatePlanarPose.h" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#pragma once

// We have to ignore warning 4005 till MSRA updates to DX11. This is a known issue with Windows 8 SDK.
#include <windows.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <fstream>
#include "GoundTruthExtractionExports.h"

// Declare reference to class so that it is exported in the dll.
class CVisDVector;

/// <summary>
/// Interface for planar pose estimator
/// </summary>
class IPlanarPoseEstimator
{
public:
    /// <summary>
    /// Estimate planar translation and rotation for the given image
    /// </summary>
    /// <param name="pbImageData">The image data in bytes in Gray 8 format </param>
    /// <param name="dwImageLength">The image data length in bytes </param>
    /// <param name="dwImageWidth">The image width in pixels</param>
    /// <param name="dwImageHeight">The image height in pixels</param>
    /// <param name="pdlTranslation">The translation in x, y, z directions of the planar checker board in mm</param>
    /// <param name="cbTranslation">The length of translation buffer, should be 3</param>
    /// <param name="pdlRotation">The rotation in x, y, z directions of the planar checker board in radians</param>
    /// <param name="cbRotation">The length of rotation array, should be 3</param>
    /// <returns>True if the estimation was successful, False otherwise</returns>
    GROUNDTRUTHEXTRACTION_DLLEXPORTIMPORT virtual bool EsimatePlanarPose(
        BYTE *pbImageData,
        DWORD dwImageLength,
        DWORD dwImageWidth,
        DWORD dwImageHeight,
        double* pdlTranslation, 
        DWORD cbTranslation,
        double* pdlRotation,
        DWORD cbRotation) = 0;
};

/// <summary>
/// MSR implementation of planar pose estimator.
/// This class is responsible for estimating the translation and rotation of the checkerboard
/// with respect to the camera with given intrinsic and distortion parameters.
/// </summary>
class MSRPlanarPoseEstimator : public IPlanarPoseEstimator {
public:
    /// <summary>
    /// Initializes a new instance of the MSRPlanarPoseEstimator class.
    /// </summary>
    /// <param name="pdlColorIntrinsicParams">
    ///  Camera intrinsic parameters.
    ///  For more information refer to http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html
    /// <param name="pdlColorIntrinsicParams[0]">
    /// Color Intrinsic Parameter 1 for camera.
    /// This is the Focal Length in x-direction (Fx) of the camera in pixels.
    /// For more information refer to http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html
    /// </param>
    /// <param name="pdlColorIntrinsicParams[1]">Color Intrinsic Parameter 2 for camera. This is the Skew of the camera.</param>
    /// <param name="pdlColorIntrinsicParams[2]">Color Intrinsic Parameter 3 for camera. This is the Focal Length in y-direction (Fy) of the camera in pixels.</param>
    /// <param name="pdlColorIntrinsicParams[3]">Color Intrinsic Parameter 4 for camera. This is the position of the principle x-axis of the camera in pixels.</param>
    /// <param name="pdlColorIntrinsicParams[4]">Color Intrinsic Parameter 5 for camera. This is the position of the principle y-axis of the camera in pixels.</param>
    /// <param name="cbColorIntrinsicParams">Camera intrinsic parameters length, should be 5</param>
    /// <param name="pdlColorRDistortionParams">  
    ///     Color Radial and Tangential Distortion Parameter 1 for camera.
    ///     For more information refer to http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html
    /// </param>
    /// <param name="cbColorIntrinsicParams">Camera radial and tangential distortion parameters length, should be 5</param>
    GROUNDTRUTHEXTRACTION_DLLEXPORTIMPORT MSRPlanarPoseEstimator(
        double* pdlColorIntrinsicParams,
        int cbColorIntrinsicParams,
        double* pdlColorRDistortionParams,
        int cbColorRDistortionParams);

    /// <summary>
    /// Destroys an instance of the MSRPlanarPoseEstimator class.
    /// </summary>
    GROUNDTRUTHEXTRACTION_DLLEXPORTIMPORT virtual ~MSRPlanarPoseEstimator();

    /// <summary>
    /// Estimate planar translation and rotation for the given image
    /// Rotation is output as (Euler) angles.
    /// </summary>
    /// <param name="pbImageData">The image data in bytes in Gray 8 format </param>
    /// <param name="dwImageLength">The image data length in bytes </param>
    /// <param name="dwImageWidth">The image width in pixels</param>
    /// <param name="dwImageHeight">The image height in pixels</param>
    /// <param name="pdlTranslation">The translation in x, y, z directions of the planar checker board in mm</param>
    /// <param name="cbTranslation">The length of translation buffer, should be 3</param>
    /// <param name="pdlRotation">The rotation in x, y, z directions of the planar checker board in radians</param>
    /// <param name="cbRotation">The length of rotation array, should be 3</param>
    /// <returns>True if the estimation was successful, False otherwise</returns>
    GROUNDTRUTHEXTRACTION_DLLEXPORTIMPORT virtual bool EsimatePlanarPose(
        BYTE *pbImageData,
        DWORD dwImageLength,
        DWORD dwImageWidth,
        DWORD dwImageHeight,
        double* pdlTranslation, 
        DWORD cbTranslation,
        double* pdlRotation,
        DWORD cbRotation);

    /// <summary>
    /// Estimate planar translation and rotation for the given image
    /// Rotation is output as a unit quaternion.
    /// </summary>
    /// <param name="pbImageData">The image data in bytes in Gray 8 format </param>
    /// <param name="dwImageLength">The image data length in bytes </param>
    /// <param name="dwImageWidth">The image width in pixels</param>
    /// <param name="dwImageHeight">The image height in pixels</param>
    /// <param name="pdlTranslation">The translation in x, y, z directions of the planar checker board in mm</param>
    /// <param name="cbTranslation">The length of translation buffer, should be 3</param>
    /// <param name="pdlRotation">The rotation of the planar checker board, specified as a unit quaternion</param>
    /// <param name="cbRotation">The length of rotation array, should be 4</param>
    /// <returns>True if the estimation was successful, False otherwise</returns>
    GROUNDTRUTHEXTRACTION_DLLEXPORTIMPORT virtual bool EsimatePlanarPoseWithRotationAsUnitQuaternion(
        BYTE *pbImageData,
        DWORD dwImageLength,
        DWORD dwImageWidth,
        DWORD dwImageHeight,
        double* pdlTranslation, 
        DWORD cbTranslation,
        double* pdlRotation,
        DWORD cbRotation);

private:
    /// <summary>
    /// Convert quaternion to euler angles
    /// </summary>
    /// <param name="pdlQuaternion">The quaternion which represents the rotation of the planar checker board</param>
    /// <param name="cbQuaternion">The length of quaternion array, should be 4</param>    
    /// <param name="pdlRotation">The euler angles in x, y, z directions of the planar checker board in radians</param>
    /// <param name="cbRotation">The length of rotation array, should be 3</param>    
    /// <returns> True if the conversion was successful, False otherwise </returns>
    bool MSRPlanarPoseEstimator::QuaternionToEulerAngles(double *pdlQuaternion, DWORD cbQuaternion, double *pdlRotation, DWORD cbRotation);

    /// <summary>
    /// Normalize quaternion by dividing all 4 values by the L2 norm of the vector, resulting in a unit quaternion.
    /// </summary>
    /// <param name="pdlQuaternion">The quaternion which represents the rotation of the planar checker board</param>
    /// <param name="cbQuaternion">The length of quaternion array, should be 4</param>    
    /// <returns> True if the conversion was successful, False otherwise </returns>
    bool MSRPlanarPoseEstimator::NormalizeQuaternion(double *pdlQuaternion, DWORD cbQuaternion);
private:
    /// <summary>
    /// Camera intrinsic parameters
    /// </summary>
    CVisDVector* m_colorIntrinsicParams;

    /// <summary>
    /// Camera distortion parameters
    /// </summary>
    CVisDVector* m_colorRDistortParams;
};
